#include "VectorXYZ.h"
#define _USE_MATH_DEFINES
#include <Math.h>

#define CROSS_PRODUCT_SINGULARITY_TOLERANCE 0.999

/* *********** */
/* Constructor */

VectorXYZ::VectorXYZ(float XCoord, float YCoord, float ZCoord) {
	x = XCoord;
	y = YCoord;
	z = ZCoord;
}

/* ************** */
/* Set Operations */

void VectorXYZ::set(float XCoord, float YCoord, float ZCoord) {
	x = XCoord;
	y = YCoord;
	z = ZCoord;
}

void VectorXYZ::set(VectorXYZ v) {
	x = v.x;
	y = v.y;
	z = v.z;
}

/* **************** */
/* Other Operations */

void VectorXYZ::Normalize () {
	float inverted_mag = 1/Magnitude();
	if (inverted_mag != 0) {
		x *= inverted_mag;
		y *= inverted_mag;
		z *= inverted_mag;
	}
}

float VectorXYZ::Magnitude () {
	return sqrt((x*x + y*y + z*z));
}

VectorXYZ VectorXYZ::Proj (VectorXYZ target) {
	VectorXYZ clone(x,y,z);
	return ((clone * target) * target / (target * target));
}

VectorXYZ VectorXYZ::Perp (VectorXYZ target) {
	VectorXYZ clone(x,y,z);
	return clone - Proj(target);
}

/* ********************** */
/* Linear Transformations */

void VectorXYZ::rotate(float angle, VectorXYZ axis) {
	axis.Normalize();
	angle = angle * (float)M_PI / 180;
	float sine = sin(angle);
	float cosine = cos(angle);
	RodriguesRotate(sine, cosine, axis);
}

void VectorXYZ::rotate(VectorXYZ basis, VectorXYZ newbasis) {
	basis.Normalize();
	newbasis.Normalize();

	// Check if the two bases are sufficiently similar to avoid the n x n = 0 singularity.
	if (abs(basis * newbasis) < CROSS_PRODUCT_SINGULARITY_TOLERANCE) {
		VectorXYZ axis = basis % newbasis;
		RodriguesRotate(axis.Magnitude(), basis * newbasis, axis);
	}
	else if (basis * newbasis < -CROSS_PRODUCT_SINGULARITY_TOLERANCE) {
		VectorXYZ temp(x,y,z);
		set(temp - 2 * temp.Proj(basis));
	}
}

void VectorXYZ::RodriguesRotate(float sine, float cosine, VectorXYZ axis) {	
	axis.Normalize();
	VectorXYZ v(x, y, z);
	VectorXYZ r = v * cosine + (axis % v) * sine + axis * (axis * v) * (1 - cosine);
	set(r);
}

/* ***************** */
/* Vector Operations */

VectorXYZ operator+ (VectorXYZ &v1, VectorXYZ &v2) {
	VectorXYZ temp;

	temp.x = v1.x + v2.x;
	temp.y = v1.y + v2.y;
	temp.z = v1.z + v2.z;
	
	return temp;
}

VectorXYZ operator- (VectorXYZ &v1, VectorXYZ &v2) {
	VectorXYZ temp;

	temp.x = v1.x - v2.x;
	temp.y = v1.y - v2.y;
	temp.z = v1.z - v2.z;

	return temp;
}

float operator* (VectorXYZ &v1, VectorXYZ &v2) { // Dot Product
	return ((v1.x * v2.x) + 
			(v1.y * v2.y) + 
			(v1.z * v2.z));
}

VectorXYZ operator% (VectorXYZ &v1, VectorXYZ &v2) { // Cross Product
	VectorXYZ temp;

	temp.x = (v1.y * v2.z) - (v1.z * v2.y);
	temp.y = (v1.z * v2.x) - (v1.x * v2.z);
	temp.z = (v1.x * v2.y) - (v1.y * v2.x);

	return temp;
}

///

void operator+= (VectorXYZ &v1, VectorXYZ &v2) {
	v1.x += v2.x;
	v1.y += v2.y;
	v1.z += v2.z;
}

void operator-= (VectorXYZ &v1, VectorXYZ &v2) {
	v1.x -= v2.x;
	v1.y -= v2.y;
	v1.z -= v2.z;
}

void operator%= (VectorXYZ &v1, VectorXYZ &v2) { // Cross Product
	v1.x = (v1.y * v2.z) - (v1.z * v2.y);
	v1.y = (v1.z * v2.x) - (v1.x * v2.z);
	v1.z = (v1.x * v2.y) - (v1.y * v2.x);
}

VectorXYZ operator- (VectorXYZ &v1) {
	return VectorXYZ(-v1.x, -v1.y, -v1.z);
}

/* ***************** */
/* Scalar Operations */

VectorXYZ operator* (VectorXYZ v1, float num) {
	VectorXYZ temp;

	temp.x = v1.x * num;
	temp.y = v1.y * num;
	temp.z = v1.z * num;

	return temp;
}

VectorXYZ operator/ (VectorXYZ v1, float num) {
	VectorXYZ temp;

	temp.x = v1.x / num;
	temp.y = v1.y / num;
	temp.z = v1.z / num;

	return temp;
}

///

VectorXYZ operator* (float num, VectorXYZ v1) {
	VectorXYZ temp;

	temp.x = v1.x * num;
	temp.y = v1.y * num;
	temp.z = v1.z * num;

	return temp;
}

///

void operator*= (VectorXYZ &v1, float num) {
	v1.x *= num;
	v1.y *= num;
	v1.z *= num;
}

void operator/= (VectorXYZ &v1, float num) {
	v1.x /= num;
	v1.y /= num;
	v1.z /= num;
}

/* ************* */
/* Output Stream */

std::ostream &operator<< (std::ostream &output, VectorXYZ &v) {
	output << v.x << ", " << v.y << ", " << v.z;

	return output;
}